/**
 *
 * SimSchedulerUrgent 
 * within TOSSIM events. 
 * Based on SimSchedulerBasic
 *
 * @author Mina Jung
 * @date   October  2010
 */


#include <sim_event_queue.h>

module SimSchedulerUrgentP {
  provides interface Scheduler;
  provides interface TaskBasic[uint8_t id];
  provides interface TaskUrgent[uint8_t id];
  uses interface McuSleep;
  uses interface Atm128EnergyHandler as Energy;
}
implementation
{
  enum
  {
    NUM_UTASKS = uniqueCount("TinySchedulerC.TaskUrgent"),
    NUM_TASKS = uniqueCount("TinySchedulerC.TaskBasic"),
    NO_TASK = 255,
  };

  uint8_t n_task;

  uint8_t b_queue[NUM_TASKS];
  uint8_t b_head;
  uint8_t b_tail;

  uint8_t p_queue[NUM_UTASKS];
  sim_time_t d_queue[NUM_UTASKS];
  uint8_t j_queue[NUM_UTASKS];


  bool sim_scheduler_event_pending = FALSE;
  sim_event_t sim_scheduler_event;

  int sim_config_task_latency() {return 100;}
  

  void sim_scheduler_submit_event() {
    if (sim_scheduler_event_pending == FALSE) {
      sim_time_t temp = sim_time();
      temp.ticks += sim_config_task_latency();
      temp.sec += temp.ticks/sim_ticks_per_sec();
      temp.ticks = temp.ticks%sim_ticks_per_sec();

      sim_scheduler_event.time = temp;
      sim_queue_insert(&sim_scheduler_event);
      sim_scheduler_event_pending = TRUE;
    }
  }

  void sim_scheduler_event_handle(sim_event_t* e) {
    sim_scheduler_event_pending = FALSE;

    
    if (call Scheduler.runNextTask()) {
      sim_scheduler_submit_event();
    }
  }

  
  void sim_scheduler_event_init(sim_event_t* e) {
    e->mote = sim_node();
    e->force = 0;
    e->data = NULL;
    e->handle = sim_scheduler_event_handle;
    e->cleanup = sim_queue_cleanup_none;
  }


  uint8_t calUrgency(uint8_t id1, uint8_t id2) {
    uint32_t lax1, lax2; 
    sim_time_t local = sim_time();


    lax1 = (d_queue[id1].sec - local.sec)*sim_ticks_per_sec() + (d_queue[id1].ticks - local.ticks);
    lax2 = (d_queue[id2].sec - local.sec)*sim_ticks_per_sec() + (d_queue[id2].ticks - local.ticks);

    if(lax1 < 20000)
      return id1;

    if(lax2 < 20000)
      return id2;

    if(p_queue[id1] > p_queue[id2])
      return id1;
    else if(p_queue[id1] == p_queue[id2] && j_queue[id1] >= j_queue[id2])
      return id1;
    else
      return id2;
  }

  uint8_t popUTask()
  {
    int i;
    uint8_t id = NO_TASK;

    sim_time_t temp;
    temp.sec = 0;
    temp.ticks = 0;

    if( get_current_battery(sim_node()) < 100.0 )
    {
      for( i = 0; i < NUM_UTASKS; i++)
      {
        if(p_queue[i] == NO_TASK)
          continue;

        p_queue[i] = NO_TASK;
        j_queue[i] = NO_TASK;
        d_queue[i] = temp;

      }

      for( i = 0; i < NUM_TASKS; i++)
      {
        if(b_queue[i] == NO_TASK)
          continue;

        b_queue[i] = NO_TASK;
      }

      n_task = 0;
      b_head = NO_TASK;
      b_tail = NO_TASK;


      return NO_TASK;
    }


    if( n_task > 0)
    {
      // pick highest urgent task
  
      for( i = 0; i < NUM_UTASKS; i++)
      {
        if(p_queue[i] == NO_TASK)
          continue;

        if(id == NO_TASK)
        {
          id = i;
        }
        else
        {
          id = calUrgency(id, i);
        }

      }

      p_queue[id] = NO_TASK;
      j_queue[id] = NO_TASK;
      d_queue[id] = temp;

      n_task--;

      return id;
    }
    else 
      return NO_TASK;
  }

  uint8_t popBTask()
  {
    uint8_t id = NO_TASK;
   
    if( b_head != NO_TASK )
    {
      id = b_head;
      b_head = b_queue[b_head];
      if(b_head == NO_TASK )
      {
        b_tail = NO_TASK;
      }

      b_queue[id] = NO_TASK;

      return id;

    }
    else
      return NO_TASK;
  }


  
  bool isWaiting( uint8_t id )
  {
    return (p_queue[id] != NO_TASK);
  }

  bool pushTask( uint8_t id , uint16_t priority, uint16_t deadline, uint16_t job)
  {
   
    dbg("Scheduler", "\n");

    if( get_current_battery(sim_node()) < 100.0)
    {
      return FALSE;
    }


    if( !isWaiting(id) )
    {
      sim_time_t local = sim_time();
      local.ticks += (deadline/1000.0)*sim_ticks_per_sec();
      local.sec += local.ticks/sim_ticks_per_sec();
      local.ticks = local.ticks%sim_ticks_per_sec();

      // deadline is in ms

      p_queue[id] = priority;
      d_queue[id] = local;
      j_queue[id] = job;

      n_task++;


      return TRUE;
    }
    else
    {
      return FALSE;
    }
  }

  bool isBWaiting( uint8_t id )
  {
    return (b_queue[id] != NO_TASK) || (b_tail == id);
  }


  bool pushBTask( uint8_t id )
  {
   
    if( get_current_battery(sim_node()) < 100.0)
    {
      return FALSE;
    }


    if( !isBWaiting(id) )
    {
      if(b_head == NO_TASK)
      {
        b_head = id;
        b_tail = id;
      }
      else
      {
        b_queue[b_tail] = id;
        b_tail = id;
      }
      return TRUE;
    }
    else
    {
      return FALSE;
    }
  }
  
  command void Scheduler.init()
  {
    sim_time_t temp;


    dbg("Scheduler", "Initializing scheduler.\n");
    dbg("Scheduler", "Urgent Task %hhu Basic Task %hhu\n", NUM_UTASKS, NUM_TASKS);
    atomic
    {
      int i; 

      temp.sec = 0;
      temp.ticks = 0;
     
      memset( b_queue, NO_TASK, sizeof(b_queue) );

      memset( p_queue, NO_TASK, sizeof(p_queue) );
      memset( j_queue, NO_TASK, sizeof(j_queue) );

      for(i = 0; i < NUM_UTASKS; i++)
        d_queue[i] = temp;
   
      n_task = 0;

      b_head = NO_TASK;
      b_tail = NO_TASK;

      sim_scheduler_event_pending = FALSE;
      sim_scheduler_event_init(&sim_scheduler_event);
    }
  }
  
  command bool Scheduler.runNextTask()
  {
    uint8_t nextUTask = NO_TASK;
    uint8_t nextBTask = NO_TASK;


    atomic
    {
      nextUTask = popUTask();
      if( nextUTask == NO_TASK )
      {
	dbg("Scheduler", "Told to run next task, but no Urgent task to run.\n");

        nextBTask = popBTask();

        if( nextBTask == NO_TASK )
        {
	  dbg("Scheduler", "Told to run next task, but no Urgent& Basic task to run.\n");
      
	  call McuSleep.sleep();
	  return FALSE;
        }
      }
    }

    if( nextUTask != NO_TASK )
    {
      dbg("Scheduler", "Running urgent task %hhu.\n", nextUTask);

      call Energy.mcu_state_change(6);

      signal TaskUrgent.runTask[nextUTask]();
    }
    else if( nextBTask != NO_TASK )
    {
      dbg("Scheduler", "Running Basic task %hhu.\n", nextBTask);

      call Energy.mcu_state_change(6);

      signal TaskBasic.runTask[nextBTask]();
    }

    return TRUE;
  }

  command void Scheduler.taskLoop() {
    // This should never run.
  }
  
  /**
   * Return SUCCESS if the post succeeded, EBUSY if it was already posted.
   */
  
  async command error_t TaskUrgent.postTask[uint8_t id](uint16_t priority, uint16_t deadline, uint16_t job)
  {
    error_t result;

    atomic {
      result =  pushTask(id, priority, deadline, job) ? SUCCESS : EBUSY;
    }


    if (result == SUCCESS) {
      dbg("Scheduler", "Posting Urgent task %hhu.\n", id);

      sim_scheduler_submit_event();
    }
    else {
      dbg("Scheduler", "Posting Urgent task %hhu, but already posted.\n", id);
    }

    return result;
  }

  default event void TaskUrgent.runTask[uint8_t id]()
  {
  }


  async command error_t TaskBasic.postTask[uint8_t id]()
  {
    error_t result;

    atomic {
      result =  pushBTask(id) ? SUCCESS : EBUSY;
    }


    if (result == SUCCESS) {
      dbg("Scheduler", "Posting Basic task %hhu.\n", id);

      sim_scheduler_submit_event();
    }
    else {
      dbg("Scheduler", "Posting Basic task %hhu, but already posted.\n", id);
    }
return result;
  }

  default event void TaskBasic.runTask[uint8_t id]()
  {
  }

}

